/*
    Copyright 2010-2011 Patrik Jonsson, sunrise@familjenjonsson.org

    This file is part of Sunrise.

    Sunrise is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 3 of the License, or
    (at your option) any later version.

    Sunrise is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Sunrise.  If not, see <http://www.gnu.org/licenses/>.

*/

/** \file

    Definitions of the functions used to interface with Arepo. */

// $Id$

// include order is picky here. something in the arepo headers appear
// to clobber the blitz headers, so include all non-arepo headers
// first.

#include "mcrx-arepo.h"
#include "blitz/numinquire.h"
#include "mcrx-debug.h"
#include "misc.h"
#include "units.h"
#include "constants.h"
#include "mono_poly_abstract.h"
#include "vecops.h"

// KLUDGE: we don't need these two here but they are included by arepo
// and they won't compile in C-linkage mode
#include "mpi.h"
#include "gmp.h"
extern "C" {
#include "arepoconfig.h"
#include "proto.h"
}

// These are declared here because they expose the Arepo data structures.
namespace arepo {
  /** A pointer to the main Arepo tessellation object. */
  tessellation* T;

  int find_next_tetra(const mcrx::ray_base& r, int t, point& ppstart, 
		      point& ppend, point& ppexit, 
		      int& next_tetra, int& previous_tetra);
  void set_point(const mcrx::vec3d& p, point& pptr, bool si=true);
  mcrx::vec3d get_point(const point& pptr);
};

using namespace std;
using mcrx::vec3d;
using mcrx::vec3i;
using mcrx::T_float;

using arepo::arepo_units;
using arepo::lcon;

/** The units that the Arepo routines return their quantities
    as. (This is the Arepo quantities multiplied by the appropriate
    *con conversion factors below.) */
mcrx::T_unit_map arepo::arepo_units;
T_float arepo::lcon;
T_float arepo::mcon;
T_float arepo::tcon;

/** This vectors maps dp_idx to the dp_idx of the primary cell. */
std::vector<int> arepo::primary_cells;
/** This vector stores the starting midpoint and the number of faces
    for the cell, indexed by dp_idx. */
std::vector<std::pair<int, int> > arepo::midpoint_idx;
/** This vector stores the midpoints between the mesh points, indexed
    by midpoint_idx. */
std::vector<mcrx::vec3d> arepo::midpoints;
/** This vector stores the dp index of the cell opposite to the
    midpoint. */
std::vector<int> arepo::opposite_points;

mcrx::vec3d arepo::arepomin;
mcrx::vec3d arepo::arepomax;

/** Perform Arepo initialization. Currently, this just calls MPI_Init. */
void arepo::init(int* pargc, char*** pargv) {
  // Initialize MPI if we're using Arepo
  MPI_Init(pargc, pargv);
};


/** Sets the Arepo point to the specified vec3d. Converts to internal
    arepo units. */
void arepo::set_point(const mcrx::vec3d& p, point& pptr, bool si) {
  pptr.x = p[0]/lcon; pptr.y = p[1]/lcon;   pptr.z = p[2]/lcon;
  if(si) 
    set_integers_for_pointer(&pptr);
}

/** Converts the point in the Arepo DP array to a vec3d, converting
    to specified length units. */      
mcrx::vec3d arepo::get_point(const point& pptr) {
  return mcrx::vec3d(pptr.x, pptr.y, pptr.z)*lcon;
}  

/** Converts the point in the Arepo DP array to a vec3d, converting
    to specified length units. */      
mcrx::vec3d arepo::get_point(int p) {
  point* pptr=&T->DP[p];
  return mcrx::vec3d(pptr->x, pptr->y, pptr->z)*lcon;
}  

/** Returns true if the mesh point DP[dp_idx] is a primary cell. */
bool arepo::primary_cell(int dp_idx) {
  return Mesh.DP[dp_idx].task == ThisTask && 
    Mesh.DP[dp_idx].index>=0 && Mesh.DP[dp_idx].index<N_gas;
}

/** Returns the entry into the SphP array for the specified DP mesh
    point. For primary cells, sph_idx==dp_idx, but for local ghosts it
    is dp_index - N_gas. */
int arepo::sph_index(int dp_idx) {
  if (Mesh.DP[dp_idx].task == ThisTask) {
    if(primary_cell(dp_idx))
      return Mesh.DP[dp_idx].index;
    else if (dp_idx>=N_gas) {
      // local ghost
      const int sphp_idx = Mesh.DP[dp_idx].index - N_gas;
      assert(sphp_idx>=0 && sphp_idx<N_gas);
      return sphp_idx;
    }
    else {
      // no corresponding hydro cell
      return -1;
    }
  }
  else {
    // foreign ghost - shouldn't happen
    DEBUG(1,cerr << "Error: Foreign ghost cell encountered" << endl;);
    assert(0);
  }
  return -1;
}


/** This function initializes MPI and tells Arepo to load the
    snapshot. n_threads is used to make sure that we have enough
    points in the DP array to accommodate the needs for all
    threads. */
void arepo::load_snapshot(const std::string& snapfn, 
			  const std::string& parfile, 
			  int n_threads, const mcrx::T_unit_map& units)
{
  cout << "Calling Arepo to load snapshot " << snapfn << endl;

  MPI_Comm_rank(MPI_COMM_WORLD, &ThisTask);
  MPI_Comm_size(MPI_COMM_WORLD, &NTask);

  T=&Mesh;

  // turn off arepo dumping eos and cooling curve
  WriteMiscFiles = 0;

  strcpy(ParameterFile,parfile.c_str());
  RestartFlag = SUNRISE;
  RestartSnapNum = -1;
  begrun1();
  char s[500];
  // remove possible suffix from snap name since arepo decides this
  // based on the format specified in the parfile
  strcpy(s,strip_suffix(snapfn).c_str());
  
  // load file, but only use gas particles
  read_ic(s, 0x01);
  int status=::init();
  assert(status==SUNRISE);
  cout << "Arepo returned successfully\n  Mesh contains " << T->Ndp
	    << " Delaunay points, " << N_gas << " hydro cells\n";

  // This was done in make_3d_voronoi_projected_image
  T->DTC = reinterpret_cast<tetra_center*>(mymalloc_movable(&T->DTC, "DTC", 
							    T->MaxNdt * sizeof(tetra_center)));
  T->DTF = reinterpret_cast<char*>(mymalloc_movable(&T->DTF, "DTF",
						    T->MaxNdt * sizeof(char)));

  tetra_center *DTC = T->DTC;
  char *DTF = T->DTF;
  point *DP = T->DP;
  tetra *DT = T->DT;
  
  for(int i = 0; i < T->Ndt; i++)
    DTF[i] = 0;

  compute_circumcircles(T);

  // init units and define conversion factors
  // we need to include h-inverse here
  arepo_units["length"] = units.find("length")->second;
  arepo_units["time"] = units.find("time")->second;
  arepo_units["mass"] = units.find("mass")->second;
  
  const T_float hinv=1./All.HubbleParam;
  lcon = units::convert("cm",arepo_units["length"])*All.UnitLength_in_cm*hinv;
  mcon = units::convert("g",arepo_units["mass"])*All.UnitMass_in_g*hinv;
  tcon = units::convert("s",arepo_units["time"])*All.UnitTime_in_s*hinv;

  arepomin=0;
  arepomax=boxSize*lcon;

  // build the streamlined data structure for Voronoi planes.
  calculate_midpoints();

  // Apply additional gradient limit to ensure the density field is
  // non-negative everywhere.
  ensure_positive_densities();
}


/** Locates the nearest mesh point through a brute-force search of all
    mesh points. This is only used for debugging to avoid relying on
    the possibly-broken mesh connectivity graph. */
int arepo::locate_brute(const mcrx::vec3d& p)
{
  const point* const DP = arepo::T->DP;
  vec3d celldist(p-arepo::get_point(0));
  double dist2min = dot(celldist, celldist);
  int dp_idx=0;
  for(int i=1; i<arepo::T->Ndp; ++i) {
    celldist = p-arepo::get_point(i);

    const double dist2 = dot(celldist, celldist);
    if(dist2 < dist2min) {
      dist2min = dist2;
      dp_idx = i;
    }
  }

  DEBUG(2,cout << "\tFound " << dp_idx << endl;);
  return dp_idx;
}


/** Sets up the midpoints array which stores the midpoints of the face
    planes in a compact way so we don't have to chase a bunch of
    pointers all over the place to do the intersection tests with the
    face planes when finding the voronoi neighbors. The connections
    stored in our data structure differs from that in the Arepo DC
    array in that we map connections between distinct mesh points, not
    just primary cells. */
void arepo::calculate_midpoints()
{
  cout << "Setting up Voronoi cell connectivity structures\n";
  const point* const DP = T->DP;

  // The first thing we must do is to set up a temporary multimap of
  // sphp_idx->dp_idx, so we can identify all local ghosts associated
  // with a sph entry.
  cout << "\tReverse-mapping SPH to DP entries to find local ghosts" << endl;
  typedef multimap<int, int> mmint;
  mmint sphp_dp_map;
  for(int dp_idx=0; dp_idx<T->Ndp; ++dp_idx) {
    const int sphp_idx=sph_index(dp_idx);
    sphp_dp_map.insert(make_pair(sphp_idx, dp_idx));
  }

  // Now that we have the sphp->dp mapping, we can set up the mapping
  // from dp indices to the dp indices of the primary cells.
  for(int dp_idx=0; dp_idx<T->Ndp; ++dp_idx) {
    const int sphp_idx=sph_index(dp_idx);
    if(sphp_idx<0)
      // cell has no hydro quantities, so this doesn't make sense
      primary_cells.push_back(-1);

    // loop over all dp indices that share this sphp cell
    pair<mmint::iterator, mmint::iterator> dp_indices
      (sphp_dp_map.equal_range(sphp_idx));
    assert(dp_indices.second != dp_indices.first);
    while( dp_indices.first != dp_indices.second) {
      // when we find the primary cell, save it
      if (primary_cell(dp_indices.first->second)) {
	primary_cells.push_back(dp_indices.first->second);
	break;
      }
      ++dp_indices.first;
    }
  }
  assert(primary_cells.size()==T->Ndp);

  // use VF array to generate connections. this is just a matter of
  // reorganizing it to index by point.
  cout << "\tBuilding connection multimap" << endl;
  mmint connections;
  for(int i=0;i<T->Nvf; ++i) {
    connections.insert(make_pair(T->VF[i].p1, T->VF[i].p2));
    connections.insert(make_pair(T->VF[i].p2, T->VF[i].p1));
  }
  cout << "\tAssociating connections with cells" << endl;
  for(int dp_idx=0; dp_idx<T->Ndp; ++dp_idx) {
    // check if it's a valid cell
    if(sph_index(dp_idx)<0)
      continue;

    const vec3d cell_p(get_point(dp_idx));

    // Find the connections connecting to this cell
    pair<mmint::iterator, mmint::iterator> dp_neighbors
      (connections.equal_range(dp_idx));
    assert(dp_neighbors.first != dp_neighbors.second);

    for( ;dp_neighbors.first != dp_neighbors.second; ++dp_neighbors.first) {
      const int dp_neighbor = dp_neighbors.first->second;
      if(sph_index(dp_neighbor)<0)
	continue;

      const vec3d midpoint(0.5*(cell_p + get_point(dp_neighbor)));

      // now add the connection to the data structure
      midpoints.push_back(midpoint);
      opposite_points.push_back(dp_neighbor);
    }

    // now that we've done all the connections for DP cell dp_idx,
    // update the number and position into the midpoint index vector.
    const int start_pos = 
      midpoint_idx.empty() ? 0 :
      (midpoint_idx.back().first + midpoint_idx.back().second);
    midpoint_idx.push_back(make_pair(start_pos, midpoints.size()-start_pos));
  }
  

  cout << midpoints.size() << " connections generated" << endl;
}


/** Returns the DP point index for the cell containing the point p. If
    a negative value is returned, the point is outside the grid. The
    search only includes the local domain, if more than one processor
    is used. If is_inside is set, the check for whether the point is
    inside the box is skipped. This is necessary if the position has
    been extended to be on the box face but due to roundoff is just
    outside. */
int arepo::locate(const mcrx::vec3d& p, bool is_inside)
{
  point *DP = T->DP;
  tetra *DT = T->DT;
  const int Ndp = T->Ndp;

  if(!is_inside && !in_arepo_box(p))
    return -1;

  // We use the new tree finding function to find the closest
  // point. We need to ignore periodic boundary to not wrap around,
  // but this still doesn't get ghost points.
  vec3d pos = p*(1./arepo::lcon);

  MyFloat mindist;
  const int corig = ngb_treefind_nearest_local(&pos[0], &mindist, 0);
  assert(DP[corig].index==corig);
  mindist*=arepo::lcon; // is in Arepo units
  // if pos is near the boundary, it may lie in a ghost cell. The
  // ngbtree doesn't deal with ghost cells, so now we need to pick up
  // the search using the cell connection information, which DOES
  // include the ghost cells.

  int cc, c=corig;
  int n=0;
  while (true) {
    // if any neighbors are closer, use that instead and retry
    cc =arepo::find_closest_neighbor_in_cell(p, c);
    if( cc==c )
      break;
    c=cc;
    ++n;
  }

  DEBUG(2,if(n>4) cerr << "Warning: locate had to call find_closest_neighbor_in_cell " << n << " times\ncorig=" << corig << " mindist=" << mindist << " cfinal=" << cc << " dist=" << d << endl;);

  // if we did not end up in a primary cell, we must have iterated
  assert(primary_cell(cc) || (n!=0));

  DEBUG(2, cout << "Located point " << p << " in cell " << cc << ", SphP entry " << DP[cc].index << endl;);

  if(cc>=0)
    assert_contains(cc, p);

  return cc;
}


arepo::arepo_pressure_functor::arepo_pressure_functor() :
  // conversion factor for P -> P/k is to pressure to cgs and then
  // divide by Boltzmann's constant in erg/K.
  pcon(All.UnitPressure_in_cgs/(constants::k*units::convert("J/K","erg/K"))),
  dcon(All.UnitDensity_in_cgs*
       units::convert("g/cm^3",arepo_units["mass"]+"/"+arepo_units["length"]+"^3"))
{}


/** Returns the P/k (in cgs units!), density and metallicity in the
    grid cell at the specified position. */
pair<T_float,pair<T_float, T_float> > 
arepo::arepo_pressure_functor::operator()(const mcrx::vec3d& pos) const 
{
  const int dp_idx = locate(pos);
  const int sphp_idx= sph_index(dp_idx);
  if(sphp_idx>=0)
    return make_pair(SphP[sphp_idx].Pressure*pcon, 
		     make_pair(SphP[sphp_idx].Density*dcon, 
			       P[dp_idx].Metallicity));
  else
    // particle is outside the grid, we have no pressure info
    return make_pair(blitz::tiny(T_float()), make_pair(blitz::tiny(T_float()), 0.));
}

/** Checks whether the point is within the Arepo box. */
bool arepo::in_arepo_box(const mcrx::vec3d& p)
{
  return all(p>=arepomin) && all(p<=arepomax);
}


/* Return the DP index of the Voronoi cell bordering dp_idx in the
   direction of the ray r. len is set to the length to the
   intersection point. If the ray exits the Arepo box before hitting a
   cell boundary, next is set to -1 and len is the length to the
   boundary.  */
int arepo::find_next_cell(const mcrx::ray_base& r, int dp_idx, 
			  int previous, mcrx::T_float& len)
{
  point * const DP = T->DP;
  const int sphp_idx = DP[dp_idx].index;

  const vec3d p(r.position());
  const vec3d d(r.direction());
  const vec3d cell_p(get_point(dp_idx));

  assert_contains(dp_idx, p);

  // init next to -1, which means it didn't cross into another cell
  int next = -1;
  // initialize length to huge
  len = blitz::huge(T_float());

  const pair<int, int> edge = midpoint_idx[dp_idx];

  for(int i=edge.second-1; i>=0; --i) {
    // don't test the face we arrived through, if any
    if(opposite_points[edge.first+i] == previous && previous != -1)
      continue;

    // c is the vector from entry point p0 to the point on the face
    const vec3d c(midpoints[edge.first+i] - p);
    
    /* q is (half) the edge vector to the neighboring cell, which is a
       normal vector of the plane */
    const vec3d q(midpoints[edge.first+i] - cell_p);

    // sanity check: by construction we know that the point is inside
    // the cell, which means that c.q>0. If this is not true, it's
    // because some numerical error has put the point on the other
    // side of the face. In this case we short-circuit the process and
    // say it is on the face, the propagation distance is zero (if
    // it's heading towards the face, ie d.q>0). We must also guard
    // against the case where the ray is perfectly on the face, in
    // which case we will get NaN. In that case we simply ignore the
    // face.
    T_float cdotq = dot(c,q);
    T_float ddotq = dot(d,q);

    if(cdotq==0 && ddotq==0)
      continue;

    T_float s;
    if(cdotq>0) {
      // s = c.nq / d.q
      // This is the standard formula for the intersection between a
      // ray and a plane. s is the point where the ray p0+s*dir
      // intersects the plane which is perpendicular to q and goes
      // through c, i.e. the Voronoi face corresponding to the j-k
      // edge. 
      s=cdotq/ddotq;
    }
    else {
      cerr << "Warning: point on wrong side of face " 
	   << opposite_points[edge.first+i] << " in dp " 
	   << dp_idx << "\n";
      if(ddotq>0) {
	// it's heading toward the face, set s=0.
	assert(cdotq>-1e-5);
	s=0;
      }
      else
	// it's heading into the cell, ignore the face
	continue;
    }


    if(s >= 0 && s < len) {
      /* The ray intersects the Voronoi face closer to the starting
	 point than any previous points. This is our current candidate
	 next dp_idx. */
      
      next = opposite_points[edge.first+i];
      len=s;
    }
  }

  vec3d pexit(p+len*d);
  const bool none =  (len == blitz::huge(T_float()));
  if(none || !in_arepo_box(pexit)) {
    // Either we had no valid intersections (this can happen if the
    // ray is in a local ghost and heads towards the box boundary,
    // since they are not tessellated completely) or the intersections
    // were outside the box boundary. in this case we just intersect
    // the box.
    assert(! (none && primary_cell(dp_idx)));
    len = r.intersect_cube(arepomin, arepomax, true);
    pexit = p+len*d;
    if(none) {
      DEBUG(1,cerr<< "No valid intersections found, distance to box " << len << endl;);
    }
    next = -1;
  }

  arepo::assert_contains(dp_idx, pexit);
  if(next>=0) 
    arepo::assert_contains(next, pexit);
  
  return next;
}


/** Returns the dp index of the point out of the specified point and
    all its voronoi neighbors that is closest to the point p. The
    function works ok with ghost points. */
int arepo::find_closest_neighbor_in_cell(const mcrx::vec3d& p, int dp_idx)
{
  const point* const DP = T->DP;

  vec3d celldist(p-get_point(dp_idx));

  double dist2min = dot(celldist, celldist);

  const int start_edge = midpoint_idx[dp_idx].first;
  const int n_edges = midpoint_idx[dp_idx].second;

  DEBUG(2,cout<<"Testing for closest neighbor of cell " << dp_idx << endl;);
  
  for(int i=0; i < n_edges; ++i) {
    const int dp_neighbor = opposite_points[start_edge+i];

    DEBUG(3,cout<<"\tedge " << i << ", neighbor " << dp_neighbor << endl;);

    celldist = p-get_point(dp_neighbor);

    const double dist2 = dot(celldist, celldist);
    if(dist2 < dist2min) {
      dist2min = dist2;
      dp_idx = dp_neighbor;
    }
  }

  DEBUG(2,cout << "\tFound " << dp_idx << endl;);
  return dp_idx;
}


/** Solve for the fraction of the distance to get a specified fraction
    of the total column density along the entire segment through a
    single Voronoi cell. The dp_idx defines the cell, fn is the column
    density fraction, pos and dir are the position and direction of
    the start of the line segment.

    The fraction depends on the length of the total segment L and the
    quantity alpha = (density at starting point)/((density gradient
    . direction) * L).

    By integrating the column density along the ray, we get that the
    fractional distance is given by the solution to the quadratic
    equation

    fl = -alpha +- sqrt( alpha^2 + (2*alpha+1)*fn )

*/
mcrx::T_float
arepo::column_to_distance_fraction(int dp_idx, mcrx::T_float fn, 
				   const mcrx::vec3d& pos, 
				   const mcrx::vec3d& dir, mcrx::T_float L) 
{
  using arepo::lcon;
  assert(T->DP[dp_idx].task==ThisTask);
  assert(L>0);
  assert(fn>=0);
  assert(fn<=1);
  assert_contains(dp_idx, pos);
  assert_contains(dp_idx, pos+dir*L);
  const int sphp_idx = sph_index(dp_idx);

  // Because we need the delta from the point in the cell to the
  // center of mass, and this exists only for primary cells in the
  // SphP array, we need to transform the position relative to the
  // mesh point from any local ghost we may be in to the primary cell.
  const vec3d cell_center(SphP[sphp_idx].Center[0]*lcon,
			  SphP[sphp_idx].Center[1]*lcon,
			  SphP[sphp_idx].Center[2]*lcon);

  const vec3d delta_center = 
    pos - get_point(dp_idx) + get_point(primary_cells[dp_idx]) - cell_center;
  // the density units of the density gradient are arbitrary, but we
  // need to convert the units of inverse length in the gradient
  const vec3d arepo_gradient
    (vec3d(SphP[sphp_idx].Grad.drho[0],
	   SphP[sphp_idx].Grad.drho[1],
	   SphP[sphp_idx].Grad.drho[2])/lcon);

  // we don't actually need to know the sunrise densities for this
  // calculation, since (we assume) all species trace the overall
  // density. (I.e., no gradient in metallicity.)
  const T_float rho_start(SphP[sphp_idx].Density + 
			  dot(arepo_gradient, delta_center));

  const T_float alpha = rho_start/(dot(arepo_gradient, dir)*L);

  // in principle the density should never be zero if we are
  // scattering, but there are problems with the gradients that cause
  // the density to undershoot
  assert(rho_start>=0);
#ifndef NDEBUG
  if(1.+1./alpha<0) {
    // this means density undershoots zero at the end point of the segment
    cerr << "Error: Density undershoot at endpoint " << vec3d(pos+L*dir)<< " in cell " << dp_idx << ": " << cell_density(dp_idx, pos+dir*L) << endl;
    const vec3d pexit(pos+dir*L);
    const int nearest=locate_brute(pexit);
    const T_float dist = mcrx::mag(pexit-get_point(dp_idx));
    const T_float dist2 = mcrx::mag(pexit-get_point(nearest));

    cout << "Nearest cell is " << nearest << " dist " << dist2 << " but we are in " << dp_idx << " dist " << dist<< endl;
    assert(0);
  }
#endif

  if( alpha>1e10 || alpha<-1e-10 ) {
    // the gradient is vanishingly small. then we recover the normal solution
    return fn;
  }

  // solve quadratic
  const T_float f1 = -alpha;
  const T_float f2 = sqrt(alpha*alpha + (2*alpha+1)*fn);
  T_float fl=-1;

  if(f1<=0)
    // answer must be high root
    fl=f1+f2;
  else if(f1>=1)
    // must be low root
    fl=f1-f2;
  else {
    // both roots can potentially be valid
    if(f2==0) {
      // double root
      fl=f1;
    }
    else {
      // now we have to check which root it is
      T_float fa=f1+f2;
      T_float fb=f1-f2;

      bool va((fa>=0)&&(fa<=1));
      bool vb((fb>=0)&&(fb<=1));
      // make sure only one solution is valid
      assert(va!=vb);

      // pick valid solution
      fl = va ? fa : fb;
    }
  }
  
  assert((fl>=0)&&(fl<=1));
  return fl;
}


#ifndef NDEBUG
void arepo::assert_contains(int dp_idx, mcrx::vec3d pos)
{
  const T_float INSIDE_EPS=5e-4;
  const point* const DP = T->DP;

  vec3d celldist(pos - get_point(dp_idx));

  const double dist2p = dot(celldist, celldist);
  if(dist2p == 0.) 
    // if we are sitting right on top of the mesh point, we'll get NaN below
    return;
  double dist2min = dist2p;

  const int start_edge = midpoint_idx[dp_idx].first;
  const int n_edges = midpoint_idx[dp_idx].second;
  int c=-1;
  for(int i=0; i < n_edges; ++i) {
    const int dp_neighbor = opposite_points[start_edge+i];

    celldist = pos - get_point(dp_neighbor);

    const double dist2 = dot(celldist, celldist);
    if(dist2 < dist2min) {
      dist2min = dist2;
      c = dp_neighbor;
    }
  }

  const bool pass = dist2p/dist2min < 1+INSIDE_EPS;

  if(!pass) cout << "Failed assert_contains for cell " << dp_idx << ", position " << pos << ", nearest cell " << c << " by " << dist2p/dist2min-1 << endl;

  assert(pass);
};
#endif


/** Returns the density at point pos (in Sunrise units) in the cell
    specified by dp_idx. The point is assumed to be inside the cell,
    as the calculation uses the gradient within the cell, but this is
    not checked here. */
mcrx::T_float arepo::cell_density(int dp_idx, const mcrx::vec3d& pos)
{
  const int sphp_idx = sph_index(dp_idx);
  const point* const DP = T->DP;

  // cell center of mass transformed to the dp_idx cell (in arepo units)
  const vec3d cell_center
    (DP[dp_idx].x-DP[primary_cells[dp_idx]].x+SphP[sphp_idx].Center[0],
     DP[dp_idx].y-DP[primary_cells[dp_idx]].y+SphP[sphp_idx].Center[1],
     DP[dp_idx].z-DP[primary_cells[dp_idx]].z+SphP[sphp_idx].Center[2]);

  // distance from cell center (in arepo units)
  const vec3d delta_center = (1./lcon)*pos - cell_center;

  const vec3d arepo_gradient(SphP[sphp_idx].Grad.drho[0],
			     SphP[sphp_idx].Grad.drho[1],
			     SphP[sphp_idx].Grad.drho[2]);

  const T_float rho = 
    SphP[sphp_idx].Density + dot(delta_center, arepo_gradient);

  assert(rho==rho);

  return rho;
}


/** Loops over the circumsphere centers (ie Voronoi face vertices) and
    applies a further limiting of the density gradient to ensure that
    the density is non-negative at all points in the cell. (The Arepo
    gradient limiter only ensures it is non-negative at the face
    centers.) 

    Cells that extend across the box boundary are also set to zero
    density since it's complicated to get that to work right and
    doesn't make sense since we don't use periodic boundary conditions
    anyway.  */
void arepo::ensure_positive_densities()
{
  tetra* DT = arepo::T->DT;
  point* DP = arepo::T->DP;
  tetra_center* DTC = arepo::T->DTC;
  std::cout << "Ensuring positive definite density field." << std::endl;
  // Start by looping over all tetras
  for(int t=0; t<arepo::T->Ndt; ++t) {
    if( DT[t].t[0]<0 || 
	DT[t].p[0] == DPinfinity || DT[t].p[1] == DPinfinity || 
	DT[t].p[2] == DPinfinity || DT[t].p[3] == DPinfinity )
      // tetras with t[0]<0 or that contain an infinity point do not
      // have computed circumspheres. what does this mean?  It's fine,
      // either they are deleted and the infinity points are well
      // outside our box.
      continue;

    // circumsphere center in sunrise units
    vec3d dtc(DTC[t].cx, DTC[t].cy, DTC[t].cz);
    dtc *= arepo::lcon;

    // For each tetrahedron, we loop over the four vertices. 
    for(int p=0; p<4; ++p) {
      // find the cell connected to this vertex
      const int dp_idx = DT[t].p[p];
      // do we need to do this for ghost points too? I think they may
      // not be completely tessellated
      const int sphp_idx = sph_index(dp_idx);

      if(primary_cell(dp_idx) && (sphp_idx>=0) &&
	 (SphP[sphp_idx].Density>0) ) {
	// valid cell, calculate the density at the vertex

	if(!in_arepo_box(dtc)) {
	  DEBUG(3,cout << "Cell " << dp_idx << " crosses boundary -- setting to zero" << endl;)
	  SphP[sphp_idx].Density=0;
	  SphP[sphp_idx].Grad.drho[0]=0;
	  SphP[sphp_idx].Grad.drho[1]=0;
	  SphP[sphp_idx].Grad.drho[2]=0;
	  P[sphp_idx].Mass=0;
	}
	else {
	  assert_contains(dp_idx, dtc);
	  const T_float rho = cell_density(sphp_idx, dtc);
	  
	  if (rho<0) {
	    // calculate the max allowed gradient (actually we
	    // overcompensate a bit to lessen the risk of roundoff
	    // leading to a negative density anyway)
	    const T_float alpha = 
	      SphP[sphp_idx].Density/(SphP[sphp_idx].Density-rho)*(1.-1e-10);
	    
	    DEBUG(3,cout << "Applying additional slope limit by " << alpha << " for cell " << sphp_idx << " at point " << dtc << endl;);
	    
	    assert(alpha<1);
	    assert(alpha>=0);
	    // apply limit to gradient
	    SphP[sphp_idx].Grad.drho[0] *= alpha;
	    SphP[sphp_idx].Grad.drho[1] *= alpha;
	    SphP[sphp_idx].Grad.drho[2] *= alpha;
	  }
	}
      }
    }
  }
}
  
